#ifndef RSIM_SYS3D_H
#define RSIM_SYS3D_H
/* -------------------------------------------------------------------------- *
 * File: Sys3D.h                                                              *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Body3D.h"
#include "DCAAssembly3D.h"

namespace RSIM{

class Quaternion;
class Sys3D{
        private:
                Sys3D();
                Sys3D(const int&);
                Sys3D(const Sys3D&);
                
                // set the ids for each body. The Parameters of body can be set
                // by modifying this function
                void init();
        public:                
                Sys3D(const int& nbodies,const int& nproc);
                ~Sys3D();                                        
                Body3D *barray_;
                DCAAssembly3D *Asm_;                
                const int nbodies_;
                const int nproc_;
                int *ids_;
                int nassem_;
                Vect3 Gravity_;
                Vect3 ON_;
                Mat6x6 iLambda_;
                Mat6x6 Lambda_;
                Vect6 pe_;
                Matrix pinvjac_;
                Matrix null_;
                Matrix Jac_;
                
                void setQ(const double& Q);
                void setU(const double& u);
                void setQ();
                
                void initDCANodesForDCAABA();
                void initDCANodesForDCA();
                void updRotationMatrix();
                void collectJac();
                void updAngularVelocity();
                void calcJTx(const double *x);
                void calcJ();
                void calcNullSpaceProjectionMatrix();
                void calcNullSpaceProjectionMatrix(const int& asm_id);
                void calcNullSpaceProjectionMatrix(const int& start_id,
                                                   const int& end_id);
                void n_formInboardEOM();
                void n_formInboardEOM(const int& root,
                                      const int& leaf);
                
                void n_triangularizeBackward(const int& root,
                                             const int& leaf);
                                             
                void n_triangularizeForward(const int& root,
                                            const int& leaf);
                                             
                void n_substituteForward(const int& root,
                                         const int& leaf,
                                         Body3D *parent);
                                         
                // calcH2 is true only if handle 2 of body with id "leaf" is constrained
                // on the second handle
                void n_calcDCAHandleEquationsUsingABA_Opt(const int& root,
                                                          const int& leaf,
                                                          const int& nodeid,
                                                          const bool& calcH2 = true,
                                                          const bool& nbody = true);
                                                          
                void n_sweepForwardLillyOrin_Opt(const int& root,
                                                 const int& leaf);
                
                void n_calcDCAHandleEquationsUsingLillyOrin_Opt(const int& root,
                                                                const int& leaf,
                                                                const int& nodeid);
                                             
                void n_calcDCAHandleEquationsUsingDCA(const int& root,
                                                      const int& leaf,
                                                      const int& asm_start,
                                                      const bool& calcH2 = true);
                                                          
                void copyHandleEquationsIntoAssembly(const int& root,
                                                     const int& leaf,
                                                     const int& asmid,
                                                     const bool& copyH2=true);
                
                void assembleKinematicSweep(const int& root,
                                            const int& leaf);
                                            
                void setKinematicBoundaryConditions(const int *ids,
                                                    const int& nproc);
                void setKinematicBoundaryConditions();
                                                    
                void disassembleKinematicSweep(const int& root,
                                               const int& leaf);
                                                          
                void n_calcZeta12Backward(const int& root,
                                          const int& leaf);
                                          
                void n_calcZetaForward_Opt(const int& root,
                                           const int& leaf,
                                           const Body3D *parent);
                                           
                void n_formDCAHandleEquations_Opt(const int& root,
                                                  const int& leaf);
                                                  
                void solveSystemHandleEquationsCaseI_Opt();
                void solveSystemHandleEquationsCase_OpControl();
                void solveSystemHandleEquationsCaseII_Opt();
                                                  
                void n_assembleDCAHandleEquationsUsingDCA();
                
                void n_assemble_Opt(const Body3D& parent,
                                    const Body3D& child,
                                    DCAAssembly3D& Asm,
                                    const bool& calcH2=true,
                                    const bool& calcH1=true);
                
                void n_assemble_Opt(const DCAAssembly3D& parent, 
                                    const DCAAssembly3D& child,
                                    DCAAssembly3D& Asm,
                                    const bool& calcH2=true,
                                    const bool& calcH1=true);
                                    
                void n_assemble_Opt(const DCAAssembly3D& parent, 
                                    const Body3D& child,
                                    DCAAssembly3D& Asm,
                                    const bool& calcH2=true,
                                    const bool& calcH1=true);
                                    
                void n_assemble_Opt(const Body3D& parent,
                                    const DCAAssembly3D& child,                                 
                                    DCAAssembly3D& Asm,
                                    const bool& calcH2=true,
                                    const bool& calcH1=true);
                                    
                void disassembleDCAAssembly_Opt(const int& id,
                                                const bool& calcH2=true,
                                                const bool& calc_pinvjac=false);
                void disassembleDCAAssembly_Opt(const int& start_id,
                                                const int& end_id,
                                                const bool& calcH2=true,
                                                const bool& calc_pinvjac=false);
                void disassembleABAAssembly_Opt(const int& id,
                                                const bool& calcH2=true,
                                                const bool& calc_pinvjac=false);
                                                
                void calc_xtilde(const double *I,                                
                                 const double *J,
                                 double *t);
                void calc_DXDT(const double *I,
                               const double *J,
                               double *t);
                void calc_DTXD(const double *I,
                               const double *J,
                               double *t);
                
                /////////////////////////////////////////////////
                /////////////////////////////////////////////////
                // Debug functions
                
                void displayQ();
                void displayU();
                void displayUDot();
                void displayUDot1();
                void displayJac();
                void displayAngularVelocity();
                void displayLinearVelocity();
                void displayPosition();
                void displayvin();
                void displayAssemblyTreeIds();
                void displayHandleEquations(const int& id);
                void displayIds();
                void displayTau();
                                                
};

}
#endif